#include "Matrix4x3.h"
#include "GullConfiguration.h"
#include "Utils.h"
#include "Timer.h"
#include <assert.h>

#include <iostream>

using namespace gull::core;
using namespace std;



void
Matrix4x3::testClass()
{
    cout << "********************************************************" << endl << 
    "********************Computing Test**********************" << endl <<
    "********************************************************" << endl;
    Matrix4x3 m1(Vector3D(1,1,0),
                 Vector3D(1,0,0),
                 Vector3D(0,1,0),
                 Vector3D(0,0,0));
    
    Matrix4x3 m2;
    
    Matrix4x3 m3(Vector3D(2,1,0),
                 Vector3D(2,0,4),
                 Vector3D(0,3,0),
                 Vector3D(0,0,0));
    
    Matrix4x3 m4(Vector3D(1,0,0),
                 Vector3D(2,1,3),
                 Vector3D(3,0,2),
                 Vector3D(0,0,0));
    
    Vector3D v1(1,0,4);
    cout << "M1:" <<endl << m1 << endl;
    cout << "M3:" <<endl << m3 << endl;
    cout << "M1 *= M3" <<endl << (m1*=m3) <<endl;
    cout << "M1:" <<endl << m1 << endl;
    cout << "v1:" << v1 << endl;
    cout << "v1 *= M1:" <<endl << (v1*=m1) << endl;
    cout << "v1:" << v1 << endl;
    
    cout << "M4:"<< endl << m4 << endl;
    m4.transpose();
    cout << "M4 transpose:"<< endl << m4 << endl;
    cout << "M4 deternminant:" << m4.determinant() << endl;
    cout << "M4 inverse:" << endl << m4.inverse() << endl;
    Vector3D v2(1,4,0);
    cout << "v2: " << v2 << endl;
    cout << "v2*m4: " << (v2*=m4) << endl;
    Matrix4x3 m4inverse = m4.inverse();
    cout << "v2*m4.inverse: " << v2*m4inverse << endl;
    
    
    cout << endl << "********************************************************" << endl << 
    "********************Time Computing Test*****************" << endl <<
    "********************************************************" << endl<<endl;
    
    Timer t;
    
    Matrix4x3 mat(Vector3D(1,0,0),
                 Vector3D(2,1,3),
                 Vector3D(3,0,2),
                 Vector3D(0,0,0));
    
    
    Vector3D vec(1,4,0);
    cout << "vec: " << vec << endl;
    cout << "mat: " << endl <<  mat << endl;
    
    t.start();
    vec*=mat;
    t.stop();
    cout << "vec*=mat: " << vec << " in: "<< t << endl;
    
    t.start();
    Matrix4x3 matInverse = mat.inverse();
    t.stop();
    cout << "Compute mat inverse in: "<< t << endl;
    
    t.start();
    vec*=matInverse;
    t.stop();
    cout << "vec*=matInverse: " << vec <<" in: "<< t << endl;
    
    cout << endl << "********************************************************" << endl << 
    "********************************************************" << endl <<
    "********************************************************" << endl <<endl;
    
    
}




ostream&
operator<<(ostream& o,const Matrix4x3& m)
{
    o << m[0] << endl;
    o << m[1] << endl;
    o << m[2] << endl;
    o << m[3] << endl;
    return o;
}

Matrix4x3::Matrix4x3():
m_v1(1,0,0),
m_v2(0,1,0),
m_v3(0,0,1),
m_t(0,0,0)
{
#if GULL_CREATION_DESTRUCTION_DEBUG
    cout << "Matrix4x3 constructed: " << this << endl;
#endif
}


Matrix4x3::Matrix4x3(const Matrix4x3& m)
{
    if (&m != this) {
        m_v1 = m.m_v1;
        m_v2 = m.m_v2;
        m_v3 = m.m_v3;
        m_t = m.m_t;
    }
#if GULL_CREATION_DESTRUCTION_DEBUG
    cout << "Matrix4x3 constructed: " << this << endl;
#endif
}

Matrix4x3::Matrix4x3(const Vector3D& v1,const Vector3D& v2,const Vector3D& v3,const Vector3D& t):
m_v1(v1),
m_v2(v2),
m_v3(v3),
m_t(t)
{
#if GULL_CREATION_DESTRUCTION_DEBUG
    cout << "Matrix4x3 constructed: " << this << endl;
#endif   
}


Vector3D& 
Matrix4x3::operator[](unsigned int indice)
{
    Vector3D* m[] = {&m_v1, &m_v2, &m_v3, &m_t};
    return (*(m[indice]));
}

const Vector3D& 
Matrix4x3::operator[](unsigned short indice) const
{
    const Vector3D* m[] = {&m_v1, &m_v2, &m_v3, &m_t};
    return (*(m[indice]));
}

bool
Matrix4x3::operator==(const Matrix4x3& m) const
{
    return (m_v1 == m[0] &&
            m_v1 == m[1] &&
            m_v1 == m[2] &&
            m_v1 == m[3]);
}

bool
Matrix4x3::operator!=(const Matrix4x3& m) const
{
    return !((*this)==m);
}


Matrix4x3
Matrix4x3::operator*(const Matrix4x3& m) const
{
    //Slow mutliplication, too many copy constructor call
    
    Vector3D v1(m_v1[0] * m[0][0] + m_v1[1] * m[1][0] + m_v1[2] * m[2][0],
                m_v1[0] * m[0][1] + m_v1[1] * m[1][1] + m_v1[2] * m[2][1],
                m_v1[0] * m[0][2] + m_v1[1] * m[1][2] + m_v1[2] * m[2][2]);
    
    Vector3D v2(m_v2[0] * m[0][0] + m_v2[1] * m[1][0] + m_v2[2] * m[2][0],
                m_v2[0] * m[0][1] + m_v2[1] * m[1][1] + m_v2[2] * m[2][1],
                m_v2[0] * m[0][2] + m_v2[1] * m[1][2] + m_v2[2] * m[2][2]);
    
    Vector3D v3(m_v3[0] * m[0][0] + m_v3[1] * m[1][0] + m_v3[2] * m[2][0],
                m_v3[0] * m[0][1] + m_v3[1] * m[1][1] + m_v3[2] * m[2][1],
                m_v3[0] * m[0][2] + m_v3[1] * m[1][2] + m_v3[2] * m[2][2]);
    
    Vector3D v4(m_t[0]*m[0][0] + m_t[1]*m[1][0] + m_t[2]*m[2][0] + m[3][0],
                m_t[0]*m[0][1] + m_t[1]*m[1][1] + m_t[2]*m[2][1] + m[3][1],
                m_t[0]*m[0][2] + m_t[1]*m[1][2] + m_t[2]*m[2][2] + m[3][2]);
    
    return Matrix4x3(v1,v2,v3,v4);
}

Matrix4x3& 
Matrix4x3::operator*=(const Matrix4x3& m)
{
    Matrix4x3 res = (*this)*m;
    m_v1 = res[0];
    m_v2 = res[1];
    m_v3 = res[2];
    m_t = res[3];
    return (*this);
}

Vector3D 
operator*(const Vector3D& v, const Matrix4x3& m)
{
    return Vector3D(v[0]*m[0][0] + v[1]*m[1][0] + v[2]*m[2][0] + m[3][0],
                    v[0]*m[0][1] + v[1]*m[1][1] + v[2]*m[2][1] + m[3][1],
                    v[0]*m[0][2] + v[1]*m[1][2] + v[2]*m[2][2] + m[3][2]);    
}

Vector3D&
operator*=(Vector3D& v, const Matrix4x3& m)
{
    v = v*m;
    return v;
}

Matrix4x3::~Matrix4x3()
{
#if GULL_CREATION_DESTRUCTION_DEBUG
    cout << "Matrix4x3 deleted: " << this << endl;
#endif
}




Matrix4x3& 
Matrix4x3::operator=(const Matrix4x3& m)
{
    if(this != &m)
    {
        m_v1 = m[0];
        m_v2 = m[1];
        m_v3 = m[2];
        m_t = m[3];
    }
    return *this;
}


Matrix4x3
Matrix4x3::operator*(double a) const
{
    return Matrix4x3(m_v1*a,m_v2*a,m_v3*a,m_t*a);
}

Matrix4x3&
Matrix4x3::operator*=(double a)
{
    (*this) = (*this) * a;
    return (*this);
}


Matrix4x3
Matrix4x3::operator/(double a) const
{
    return Matrix4x3(m_v1/a,m_v2/a,m_v3/a,m_t/a);
}


Matrix4x3&
Matrix4x3::operator/=(double a)
{
    (*this) = (*this) / a;
    return (*this);
}



void
Matrix4x3::identity()
{
    m_v1 = Vector3D(1,0,0);
    m_v2 = Vector3D(0,1,0);
    m_v3 = Vector3D(0,0,1);
    m_t = Vector3D(0,0,0);
}


Matrix4x3& 
transpose(gull::core::Matrix4x3& m)
{
    std::swap(m[0][1], m[1][0]);
    std::swap(m[0][2], m[2][0]);
    std::swap(m[1][2], m[2][1]);
    return m;
}

void
Matrix4x3::transpose()
{
    std::swap(m_v1[1], m_v2[0]);
    std::swap(m_v1[2], m_v3[0]);
    std::swap(m_v2[2], m_v3[1]);
}

double 
Matrix4x3::determinant()
{
    const Matrix4x3& m = (*this);
    return (m[0][0] * (m[1][1]*m[2][2] - m[1][2]*m[2][1]) +
            m[0][1] * (m[1][2]*m[2][0] - m[1][0]*m[2][2]) +
            m[0][2] * (m[1][0]*m[2][1] - m[1][1]*m[2][0]));
    
}

Matrix4x3
Matrix4x3::inverse()
{
    double det = determinant();
    assert(det!=0 && "Can't invert Singular Matrix");
    
    //cofactors Matrix
    Matrix4x3 coFactors;
    const Matrix4x3& m = (*this);
    coFactors[0] = Vector3D(m[1][1] * m[2][2] - m[1][2] * m[2][1],
                            -(m[1][0] * m[2][2] - m[1][2] * m[2][0]),
                            m[1][0] * m[2][1] - m[1][1] * m[2][0]);
    
    coFactors[1] = Vector3D(-(m[0][1] * m[2][2] - m[0][2] * m[2][1]),
                            m[0][0] * m[2][2] - m[0][2] * m[2][0],
                            -(m[0][0] * m[2][1] - m[0][1] * m[2][0]));
    
    coFactors[2] = Vector3D(m[0][1] * m[1][2] - m[0][2] * m[1][1],
                            -(m[0][0] * m[1][2] - m[0][2] * m[1][0]),
                            m[0][0] * m[1][1] - m[0][1] * m[1][0]);
    
    
    coFactors/=det;
    
    coFactors[3] = -(m[3]);// * coFactors);
    coFactors.transpose();
    
    return coFactors;
}


void 
Matrix4x3::rotateX(double degree)
{
	double rad = degToRad(degree);
	Matrix4x3 rotatex = Matrix4x3(Vector3D(1.0,0.0,0.0),
								  Vector3D(0.0,cos(rad),sin(rad)),
								  Vector3D(0.0,-sin(rad),cos(rad)),
								  Vector3D(0.0,0.0,0.0));
	(*this)*=rotatex;
}

void 
Matrix4x3::rotateY(double degree)
{
	double rad = degToRad(degree);
	Matrix4x3 rotatey = Matrix4x3(Vector3D(cos(rad),0.0,-sin(rad)),
								  Vector3D(0.0,1.0,0.0),
								  Vector3D(sin(rad),0.0,cos(rad)),
								  Vector3D(0.0,0.0,0.0));
	(*this)*=rotatey;
	
}

void 
Matrix4x3::rotateZ(double degree)
{
	double rad = degToRad(degree);
	Matrix4x3 rotatez = Matrix4x3(Vector3D(cos(rad),sin(rad),0.0),
								  Vector3D(-sin(rad),cos(rad),0.0),
								  Vector3D(0.0,0.0,1.0),
								  Vector3D(0.0,0.0,0.0));
	
	(*this)*=rotatez;
}

